#include <motion_controller/motion_controller_node.h>




/**************************************************************
 * Sub
 ***************************************************************/
// Rviz位姿回调函数
void MotionController::RvizPosesCallback(const geometry_msgs::PoseStampedConstPtr& msg){
    __goal_pose_ptr = msg;
    ROS_INFO("[RvizPosesCallback] get pose, x=%0.3f, y=%0.3f, yaw=%0.3f.", 
        msg->pose.position.x, 
        msg->pose.position.y,
        tf2::getYaw(msg->pose.orientation)
    );
    // __goal_pose_pub.publish(*msg);

    // 开始进入P2P任务
    Pose2D goal = msg_convert::Pose2Pose2D(msg->pose);
    vector<Pose2D> points;
    points.emplace_back(goal);
    __controller_ptr->SetData(points);
    __controller_ptr->SetEvent(ControllerState::Event::EV_WORKING);
}



// 地图回调函数
void MotionController::MapCallback(const nav_msgs::OccupancyGridConstPtr& msg){
    __map_ptr = msg;
    ROS_INFO("Receive a map, width=%d, height=%d", __map_ptr->info.width, __map_ptr->info.height);
    
    // 简单膨胀处理
    cv::Mat map_image(__map_ptr->info.height, __map_ptr->info.width, CV_8UC1); 
    // for (int i = 0; i < __map_ptr->data.size(); ++i) {
    for (int y = 0; y < __map_ptr->info.height; ++y) {
        for (int x = 0; x < __map_ptr->info.width; ++x){
            int index = y * __map_ptr->info.width + x;
            int8_t value = __map_ptr->data[index];
            uchar pixel_value = static_cast<uchar>(value < 0 ? 128 : (255 - value / 100.0 * 255)); 
            map_image.at<uchar>(y, x) = pixel_value;     
        } 
    }
    cv::Mat __map_image;
    cv::flip(map_image, __map_image, 0);    //上下翻转    
    // 障碍物膨胀处理
    int robot_radius_pixel = int(__robot_radius / __map_ptr->info.resolution);
    cv::Mat dilate_struct = cv::getStructuringElement(0, Size(robot_radius_pixel, robot_radius_pixel));     //0 矩形结构元素 1 十字结构元素
    cv::erode(__map_image, __map_image, dilate_struct);
    // cv::imwrite("/home/xxty/trobot_code_ros1/study_ws/dilate_map.png", __map_image);

    nav_msgs::OccupancyGrid map_erode;
    map_erode.header = __map_ptr->header;
    map_erode.header.stamp = ros::Time::now();
    map_erode.info = __map_ptr->info;
    for(int h=0; h<__map_ptr->info.height; h++)
        for(int w=0; w<__map_ptr->info.width; w++){
            uchar value = __map_image.at<uchar>(__map_ptr->info.height - h, w);
            int8_t new_value = static_cast<int8_t>(value == 128 ? -1 : (100 - value / 255.0 * 100));
            map_erode.data.emplace_back(new_value);
    }

    // 转换数据格式
    // __map_2d.SetMap(*msg);
    __map_2d.SetMap(map_erode);

    // 设置地图数据
    if(__planner_bridge_ptr){
        // __planner_bridge_ptr->SetMap2D(__map_2d, __map_ptr);
        __planner_bridge_ptr->SetMap2D(__map_2d, 
            boost::make_shared<const nav_msgs::OccupancyGrid>(map_erode));
    }
}



